#include <iostream>
#include <math.h>

#include "arena_jumping_ball.h"

using namespace std;

Arena_jumping_ball::Arena_jumping_ball()
{
  reset();

  // parameters
  _mass_cart = 10;
  _g = 9.8; // gravity
  _width_cart = 2.0;

  objects = new Objects(200, 200);
  // ball
  Circle c(0, 0, 4);
  objects->add(c);
  // cart
  Rectangle r(0, 180, _width_cart * 10, 10);
  objects->add(r);
  update_arena();

}

Arena_jumping_ball::~Arena_jumping_ball()
{
}

void Arena_jumping_ball::reset(void)
{
  Arena_base::reset();
  _position = 0;
  _velocity = 0;
  _ball_x_position = 0;
  _ball_x_velocity = 0;
  _ball_y_position = 10;
  _ball_y_velocity = 0;

  _F = 0;
}

float *Arena_jumping_ball::get_position_p(void)
{
  return &_position;
}

float *Arena_jumping_ball::get_velocity_p(void)
{
  return &_velocity;
}

float *Arena_jumping_ball::get_ball_x_position_p(void)
{
  return &_ball_x_position;
}

float *Arena_jumping_ball::get_ball_y_position_p(void)
{
  return &_ball_y_position;
}

float *Arena_jumping_ball::get_ball_x_velocity_p(void)
{
  return &_ball_x_velocity;
}

float *Arena_jumping_ball::get_ball_y_velocity_p(void)
{
  return &_ball_y_velocity;
}

void Arena_jumping_ball::update_state(float time)
{
  float dt = 0.001;

  while (_time < time) {
    if (_time + dt > time) {
      dt = time - _time;
    }

    _velocity += (_F / _mass_cart) * dt;
    _position += _velocity * dt;

    _ball_x_position += _ball_x_velocity * dt;
    _ball_y_position += _ball_y_velocity * dt;
    _ball_y_velocity += -_g * dt;

    if (_ball_y_position < 0) {
      if (fabs(_position - _ball_x_position) < _width_cart / 2.0) {
        _ball_y_velocity = fabs(_ball_y_velocity);
        if(_ball_x_position < 3) {
          _ball_x_velocity = 1.5;
        } else {
          _ball_x_velocity = -1.5;
        }
      }
      else {
        _failure = true;
      }
    }
    _time += dt;
  }
}

void Arena_jumping_ball::push(float force)
{
  _F = force;
}

void Arena_jumping_ball::update(float time)
{
  if (time < _time) {
    cout << "ERROR: delta time negative" << endl;
    exit(1);
  }

  update_state(time);

  // remove forcing - will have to be re-applied from outside
  _F = 0;

  _time = time;
  update_arena();
}

void Arena_jumping_ball::update_arena(void)
{
  // ball
  objects->circles[0].p.x = 20 + _ball_x_position * 10;
  objects->circles[0].p.y = 190 - _ball_y_position * 10 - 10 - 2;
  // cart
  objects->rectangles[0].p1.x = 20 + _position * 10 - _width_cart * 10 / 2;
}

boost::function<void(float)> Arena_jumping_ball::function_set_force(void)
{
  return std::bind1st(std::mem_fun(&Arena_jumping_ball::push), this);
}
